#include "ConveyorMaster.h"

ConveyorMaster* ConveyorMaster::_instance = NULL;

ConveyorMaster::ConveyorMaster(NetworkTable *dash, DriverConsole *console) {
	_smartDash = dash;
	_driverConsole = console;
	_convRoller = new Relay(CONV_ROLLER_CHANNEL);
	_botRoller = new Jaguar(BOTTOM_ROLLER_CHANNEL);
	_topRoller = new CANJaguar(TOP_ROLLER_CHANNEL);
	_topRoller->ChangeControlMode(CANJaguar::kPercentVbus);
	printf("Initializing %d %d %d\n", TOP_LIGHT_SENSOR, MIDDLE_LIGHT_SENSOR, BOTTOM_LIGHT_SENSOR);
		 
	_tLSensor = new DigitalInput(TOP_LIGHT_SENSOR);
	_mLSensor = new DigitalInput(MIDDLE_LIGHT_SENSOR);
	_bLSensor = new DigitalInput(BOTTOM_LIGHT_SENSOR);
	printf("Before\n");
	_bLimit = new AnalogChannel(BOTTOM_LIMIT_SENSOR);
	printf("Middle\n");
	_tLimit = new AnalogChannel(TOP_LIMIT_SENSOR);
	printf("End\n");
	
	_startTime = 0;
	_firing = false;
	_topNum = 0;
	_loading = false;
	_botReady = true;
	_botDelayStart = 0;
}

ConveyorMaster::~ConveyorMaster() {
	delete _convRoller;
	delete _botRoller;
	delete _tLSensor;
	delete _mLSensor;
	delete _bLSensor;
	delete _bLimit;
	delete _tLimit;
}

ConveyorMaster* ConveyorMaster::GetInstance(NetworkTable *dash, DriverConsole *console) {
	if (_instance == NULL)
		_instance = new ConveyorMaster(dash,console);
	return _instance;
}


void ConveyorMaster::Teleop(bool turretReady) {
	//if (!_driverConsole->GetButtonState(BTN_MANUAL_OVERRIDE)) {
	if ((Timer::GetFPGATimestamp() - _botDelayStart) > DELAY_BUFFER && _botDelayStart != 0) {
		_botReady = true;
		_botDelayStart = 0;
	}
	
		
		if (_bLimit->GetVoltage() > 3 && _botReady) {
			printf("It's been clicked\n");
			_topNum++;
			_loading = true;
			_firing = false;
			_botDelayStart = Timer::GetFPGATimestamp();
			_botReady = false;
		} else if (_driverConsole->GetJoystickButton(DriverConsole::JoystickLeft,BTN_FIRE) && turretReady) {
			_firing = true;
		}
		
		if (_loading) {
			LoadingRoutine();
		} else if (_firing) {
			FiringRoutine();
		} else
			_convRoller->Set(Relay::kOff);
		
		if (_topNum <= 3 && !_firing)
			_botRoller->Set(-.25);
		else 
			_botRoller->Set(.25);
	//} else {
		// Put the manual commands here
	//}
}

void ConveyorMaster::LoadingRoutine() {
	//printf("Top Num: %d\n", _topNum);
	switch (_topNum) {
		case 1:
			//this is the sensor whose state is reversed
			if ((Timer::GetFPGATimestamp() - _startTime) < TIME_BUFFER || _startTime == 0) {
				_convRoller->Set(Relay::kForward);
			}  else {
				printf("You can stop now\n");
				_startTime = 0;
				_convRoller->Set(Relay::kOff);
				_loading = false;
				return;
			} 
			if (_bLSensor->Get() == 0) {
				printf("I see you\n");
				if (_startTime == 0) {
					printf("Starting the time\n");
					_startTime = Timer::GetFPGATimestamp();
				}	
			}
			break;
		case 2:
			//this is the sensor whose state is reversed
			if ((Timer::GetFPGATimestamp() - _startTime) < TIME_BUFFER || _startTime == 0) {
				_convRoller->Set(Relay::kForward);
			}  else {
				printf("You can stop now\n");
				_startTime = 0;
				_convRoller->Set(Relay::kOff);
				_loading = false;
				break;
			} 
			if (_mLSensor->Get() == 1) {
				printf("I see you\n");
				if (_startTime == 0) {
					printf("Starting the time\n");
					_startTime = Timer::GetFPGATimestamp();
				}	
			}
			break;
		case 3:
			//this is the sensor whose state is reversed
			if ((Timer::GetFPGATimestamp() - _startTime) < TIME_BUFFER || _startTime == 0) {
				_convRoller->Set(Relay::kForward);
			}  else {
				printf("You can stop now\n");
				_startTime = 0;
				_convRoller->Set(Relay::kOff);
				_loading = false;
				break;
			} 
			if (_tLSensor->Get() == 1) {
				printf("I see you\n");
				if (_startTime == 0) {
					printf("Starting the time\n");
					_startTime = Timer::GetFPGATimestamp();
				}	
			}
			break;
		default:
			_convRoller->Set(Relay::kOff);
	}
}

void ConveyorMaster::FiringRoutine() {
	_topRoller->Set(-1);
	if (_firingState == ConveyorMaster::goingUp) {
		if (_tLimit->GetVoltage() > 3) {
			_topNum--;
			_firingState = ConveyorMaster::goingDown;
		} else {
			_convRoller->Set(Relay::kForward);
		}
	} else if (_firingState == ConveyorMaster::goingDown) {
		if (_bLSensor->Get() == 1 || (Timer::GetFPGATimestamp() - _startTime) < TIME_BUFFER ) {
			if (_startTime == 0)
				_startTime = Timer::GetFPGATimestamp();
			_convRoller->Set(Relay::kReverse);
		} else {
			_convRoller->Set(Relay::kOff);
			_firing = false;
		}
			
	}
}

void ConveyorMaster::UpdateDash() {
	_smartDash->PutBoolean("Top Ball: ",_tLSensor->Get() ? 0 : 1);
	_smartDash->PutBoolean("Middle Ball: ",_mLSensor->Get() ? 0 : 1);
	_smartDash->PutBoolean("Bottom Ball: ",_bLSensor->Get() ? 0 : 1);
	_smartDash->PutBoolean("Top Limit : ",_bLimit->GetVoltage() ? 0 : 1);
	_smartDash->PutBoolean("Bottom Limit : ",_tLimit->GetVoltage() ? 0 : 1);
}

void ConveyorMaster::PrintDump() {
	_convRoller->Set(Relay::kForward);
	_botRoller->Set(-.25);
	static int counter = 0;
	if (counter % 1000 == 0) {
		printf("New line\n");
		printf("Top Light Sensor Value: %d\n",_tLSensor->Get());
		printf("Middle Light Sensor Value: %d\n",_mLSensor->Get());
		printf("Bottom Light Sensor Value: %d\n",_bLSensor->Get());
		printf("Bottom Limit Sensor Value: %f\n",_bLimit->GetVoltage());
		printf("Top Limit Sensor Value: %f\n",_tLimit->GetVoltage());
	}
	counter++;
}
